package note;

import lejos.nxt.*;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.*;

public class Jose {
    
    static LightSensor ls = new LightSensor(SensorPort.S1,true);
 
    public static void main(String[] args) {
        int Kp = 180;                             
        int Ki = 6;                             
        int Kd = 1280;                         
        int offset = 50;
        int integral = 0;                         
        int lastError = 0;                   
        int derivative = 0;
        

        DifferentialPilot pil= new DifferentialPilot(56,119,Motor.A,Motor.B);
        OdometryPoseProvider odo =new OdometryPoseProvider(pil);
        Pose pose;
        
        pil.setTravelSpeed(140);
        Button.waitForAnyPress();
        Motor.A.resetTachoCount();
        Motor.B.resetTachoCount();
        while(!Button.ESCAPE.isDown()){
            ls.setFloodlight(true); 
            int LightValue = ls.readValue();
            LCD.drawInt(LightValue, 0, 0);
            int error = LightValue - offset;       
            integral = integral*(3/5) + error;         
            derivative = error - lastError;     
            int turn = Kp*error + Ki*integral + Kd*derivative;
            turn=turn/100;
            pil.steer(turn);
            lastError = error;  
        }
        Button.waitForAnyPress();
        pose = odo.getPose();
        System.out.println("Position :"+pose);
        Motor.A.stop();
        Motor.B.stop();
        Button.waitForAnyPress();
    }
}